/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//DRIVE MOTORS J
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void driveMotorsJ(int J1step, int J2step, int J3step, int J4step, int J5step, int J6step, int J7step, int J8step, int J9step, int J1dir, int J2dir, int J3dir, int J4dir, int J5dir, int J6dir, int J7dir, int J8dir, int J9dir, String SpeedType, float SpeedVal, float ACCspd, float DCCspd, float ACCramp) {

  jointStep[0] = J1step;
  jointStep[1] = J2step;
  jointStep[2] = J3step;
  jointStep[3] = J4step;
  jointStep[4] = J5step;
  jointStep[5] = J6step;
  jointStep[6] = J7step;
  jointStep[7] = J8step;
  jointStep[8] = J9step;


  //FIND HIGHEST STEP & RESET CURRENT STEP
  int HighStep = jointStep[0];
  for (int i = 0; i < nJoints; i++) {
    if (jointStep[i] > HighStep) {
      HighStep = jointStep[i];
    }
    curStep[i] = 1;
  }


  int highStepCur = 0;
  float curDelay = 0;

  float speedSP = 0;
  float moveDist;

  //int J4EncSteps;

  //SET DIRECTIONS

  /// J1 ///
  if (J1dir == 1 && J1MotDir == 1) {
    digitalWrite(J1dirPin, HIGH);
  }
  if (J1dir == 1 && J1MotDir == 0) {
    digitalWrite(J1dirPin, LOW);
  }
  if (J1dir == 0 && J1MotDir == 1) {
    digitalWrite(J1dirPin, LOW);
  }
  if (J1dir == 0 && J1MotDir == 0) {
    digitalWrite(J1dirPin, HIGH);
  }
  /// J2 ///
  if (J2dir == 1 && J2MotDir == 1) {
    digitalWrite(J2dirPin, HIGH);
  }
  if (J2dir == 1 && J2MotDir == 0) {
    digitalWrite(J2dirPin, LOW);
  }
  if (J2dir == 0 && J2MotDir == 1) {
    digitalWrite(J2dirPin, LOW);
  }
  if (J2dir == 0 && J2MotDir == 0) {
    digitalWrite(J2dirPin, HIGH);
  }
  /// J3 ///
  if (J3dir == 1 && J3MotDir == 1) {
    digitalWrite(J3dirPin, HIGH);
  }
  if (J3dir == 1 && J3MotDir == 0) {
    digitalWrite(J3dirPin, LOW);
  }
  if (J3dir == 0 && J3MotDir == 1) {
    digitalWrite(J3dirPin, LOW);
  }
  if (J3dir == 0 && J3MotDir == 0) {
    digitalWrite(J3dirPin, HIGH);
  }
  /// J4 ///
  if (J4dir == 1 && J4MotDir == 1) {
    digitalWrite(J4dirPin, HIGH);
  }
  if (J4dir == 1 && J4MotDir == 0) {
    digitalWrite(J4dirPin, LOW);
  }
  if (J4dir == 0 && J4MotDir == 1) {
    digitalWrite(J4dirPin, LOW);
  }
  if (J4dir == 0 && J4MotDir == 0) {
    digitalWrite(J4dirPin, HIGH);
  }
  /// J5 ///
  if (J5dir == 1 && J5MotDir == 1) {
    digitalWrite(J5dirPin, HIGH);
  }
  if (J5dir == 1 && J5MotDir == 0) {
    digitalWrite(J5dirPin, LOW);
  }
  if (J5dir == 0 && J5MotDir == 1) {
    digitalWrite(J5dirPin, LOW);
  }
  if (J5dir == 0 && J5MotDir == 0) {
    digitalWrite(J5dirPin, HIGH);
  }
  /// J6 ///
  if (J6dir == 1 && J6MotDir == 1) {
    digitalWrite(J6dirPin, HIGH);
  }
  if (J6dir == 1 && J6MotDir == 0) {
    digitalWrite(J6dirPin, LOW);
  }
  if (J6dir == 0 && J6MotDir == 1) {
    digitalWrite(J6dirPin, LOW);
  }
  if (J6dir == 0 && J6MotDir == 0) {
    digitalWrite(J6dirPin, HIGH);
  }
  /// J7 ///
  if (J7dir == 1 && J7MotDir == 1) {
    digitalWrite(J7dirPin, HIGH);
  }
  if (J7dir == 1 && J7MotDir == 0) {
    digitalWrite(J7dirPin, LOW);
  }
  if (J7dir == 0 && J7MotDir == 1) {
    digitalWrite(J7dirPin, LOW);
  }
  if (J7dir == 0 && J7MotDir == 0) {
    digitalWrite(J7dirPin, HIGH);
  }
  /// J8 ///
  if (J8dir == 1 && J8MotDir == 1) {
    digitalWrite(J8dirPin, HIGH);
  }
  if (J8dir == 1 && J8MotDir == 0) {
    digitalWrite(J8dirPin, LOW);
  }
  if (J8dir == 0 && J8MotDir == 1) {
    digitalWrite(J8dirPin, LOW);
  }
  if (J8dir == 0 && J8MotDir == 0) {
    digitalWrite(J8dirPin, HIGH);
  }
  /// J9 ///
  if (J9dir == 1 && J9MotDir == 1) {
    digitalWrite(J9dirPin, HIGH);
  }
  if (J9dir == 1 && J9MotDir == 0) {
    digitalWrite(J9dirPin, LOW);
  }
  if (J9dir == 0 && J9MotDir == 1) {
    digitalWrite(J9dirPin, LOW);
  }
  if (J9dir == 0 && J9MotDir == 0) {
    digitalWrite(J9dirPin, HIGH);
  }

  /// CALC SPEEDS ///

  // DEFAULT MM PER SECOND //

  //calc ac dc nc steps
  for (int i = 0; i < nJoints; i++) {
    ACsteps[i] = jointStep[i] * (ACCspd * 0.01);
    DCsteps[i] = jointStep[i] * (DCCspd * 0.01);
    NCsteps[i] = jointStep[i] - (ACsteps[i] + DCsteps[i]);
    ACtime[i] = ((SpeedVal * 1000) * (ACCspd * 0.01)) * 1.3333333;
    DCtime[i] = ((SpeedVal * 1000) * (DCCspd * 0.01)) * 1.3333333;
    NCtime[i] = (SpeedVal * 1000) - ACtime[i] - DCtime[i];
    NCincr[i] = NCtime[i] / NCsteps[i];
    ACincr[i] = NCincr[i] / ACsteps[i];
    DCincr[i] = NCincr[i] / DCsteps[i];
    ACbegst[i] = ACincr[i] * ACsteps[i] * 2;
  }

  // set reference start time
  startTime = millis();


  ///// DRIVE MOTORS /////
  while (curStep[0] < jointStep[0] || curStep[1] < jointStep[1] || curStep[2] < jointStep[2] || curStep[3] < jointStep[3] || curStep[4] < jointStep[4] || curStep[5] < jointStep[5] || curStep[6] < jointStep[6] || curStep[7] < jointStep[7] || curStep[8] < jointStep[8])
  {


    // find current time given current step
    for (int i = 0; i < nJoints; i++) {
      //ac
      if (curStep[i] <= ACsteps[i]) {
        curTime[i] = curStep[i] * (ACbegst[i] - (ACincr[i] * (curStep[i] - 1)) + ACbegst[i]) / 2;
      }

      //dc
      else if (curStep[i] >= ACsteps[i] + NCsteps[i]) {
        curTime[i] = ACtime[i] + NCtime[i] + (curStep[i] * (NCincr[i] + ((DCincr[i] * (curStep[i] - 1) + NCincr[i]))) / 2);
      }

      //nc
      else {
        curTime[i] = ACtime[i] + (curStep[i] * NCincr[i]);
      }
    }

    // set drive outputs
    for (int i = 0; i < nJoints; i++) {
      if (curTime[i] >= millis() - startTime) {
        digitalWrite(motStepPin[i], LOW);
        curStep[i] = ++curStep[i];
      }
    }

    delayMicroseconds(10);


    // inc high step - reset drive output
    highStepCur = ++highStepCur;
    for (int i = 0; i < nJoints; i++) {
      digitalWrite(motStepPin[i], HIGH);
    }



  }
  //set rounding speed to last move speed
  rndSpeed = curDelay;
}
